package com.jay.example.IndoorPositioningSystem;

import android.annotation.SuppressLint;
/****
 * https://gitee.com/unw1900/Indoor_Positioning_System
 */

/*****
 * 步伐检测模块
 * 存在问题：1.缺乏防抖
 *           2.需要动态阈值
 *
 * 2020.4.29  StepDetector v2.0  :满足基本使用,计算步伐基本准确
 *            缺少防震动。
 */
class StepDetector {

    private boolean isDirectionUp = false;
    private double newData = 0, oldData = 0;
    private int continueUpCount = 0;
    private double peakOfWave;
    private double valleyOfWave;
    private double timeOfThisPeak = 0;
    private double gateValue = 1;
    private double timeOfLastPeak;
    private int tempCount;
    private final int valueNum = 4;
    private double[] tempValue = new double[valueNum];
    private int continueUpFormerCount = 0;
//    private double time12 = 0;   测试值

    private int stepsum = 0;

    public double compensator;//根据“峰峰值”补偿步长
    public double getCompensator() {
        return compensator;
    }

    /****
     * 步伐探测模块
     * @param data 经过滤波的最新数据
     * @param diff_time 数据的时间戳（ms），
     */
    @SuppressLint("SetTextI18n")
    boolean StepDetector(double data, double diff_time) {
        boolean step = false;
        if (oldData == 0) {
            oldData = data;
//            timeOfLastPeak = difftime;
        } else {
            oldData = newData;
            newData = data;
            if (DetectorPeak(newData, oldData)) {
                timeOfLastPeak = timeOfThisPeak;
                if ((diff_time - timeOfLastPeak) >= 0.33
                        && (peakOfWave - valleyOfWave >= gateValue))
                {
                    timeOfThisPeak = diff_time;
                    compensator = Peak_Valley_Thread(peakOfWave - valleyOfWave);//动态阈值，实现效果不行
                    stepsum++;
                    step = true;
                }
            }
        }
        return step;
    }
    /****
     * 波峰检测，条件：
     * 1.当前点呈下降趋势；
     * 2.上一点呈上升趋势；
     * 3.波峰到来前，经历至少三次accValue上升
     * 4.波峰值大于 1.5
     * @param newValue 最新的accValue
     * @param oldValue 上一次的accValue
     * @return 检测到波峰则返回 true；否则返回 false。
     */
    private boolean DetectorPeak ( double newValue, double oldValue){
        boolean lastStatus = isDirectionUp;
        if (newValue >= oldValue) {//当前acc为 上升趋势
            isDirectionUp = true;
            continueUpCount++;//记录上升次数
        } else {//当前acc为 下降趋势
            continueUpFormerCount = continueUpCount;//加速度持续上升次数。用于动态阈值，缺乏相关函数
            continueUpCount = 0;
            isDirectionUp = false;
        }
//        if (!isDirectionUp && lastStatus &&(continueUpFormerCount>=4 || oldData>=2.5)){
        if (!isDirectionUp && lastStatus &&         //1.当前acc趋势从上升转为下降：出现波峰
                (continueUpFormerCount>=3) &&       //2.波峰前持续上升3次
                ( oldValue>=2 || valleyOfWave<=1.5)) //3.波峰大于2，且波谷小于1.5
        {
            peakOfWave = oldValue;//记录波峰
            return true;
        }
        else if (!lastStatus && isDirectionUp)
        {//当前acc趋势从下降转为上升：出现波谷
            valleyOfWave = oldValue;//记录波谷
            return false;
        }
        else
        {
            return false;
        }
    }

    /****
     * 阈值计算：存四次差值，求出阈值*******待优化！！！！
     * @param value 波峰与波谷的差值
     * @return 最新阈值
     */
    private double Peak_Valley_Thread(double value){
        double tempThread = gateValue;
        if (tempCount < valueNum){      //保存四次波峰波谷差值
            tempValue[tempCount] = value;
            tempCount++;
        }else { //大于四次后，滑动更新差值数据，并根据最新的4组数据计算阈值
            tempThread = averageValue(tempValue);
            System.arraycopy(tempValue, 1, tempValue, 0, valueNum - 1);
            tempValue[valueNum-1] = value;
        }
        return tempThread;
    }

    /****
     * 求阈值平均值，根据平均值对阈值进行梯度化
     * @param array 输入序列
     * @return 梯度化后的阈值
     */
    private double averageValue(double[] array) {	//求均值
        double sum = 0;
        double ave;
        for (double v : array) {
            sum += v;
        }
        ave = (sum / array.length);
        //根据平均“峰峰值”，设置不同的梯度阈值
        if (ave>=2)
            ave = 1.5;
        else if (ave>=1.5 && ave <2)
            ave = 1.4;
        else if (ave>=1.2 && ave <1.5)
            ave = 1.2;
        else if (ave>=1 && ave <1.2)
            ave = 1.1;
        else {
            ave = 0.8;
        }
        return ave;
    }

}